#include <arch.h>
#include <asm_macros.h>
#include <plat.h>

 .local normal_stack
 .global normal_stack_top
 .local crash_stack
 .global crash_stack_top
 .global plat_get_core_pos
 .global plat_set_stack
 .global plat_set_crash_stack
 .global plat_handle_interrupt
 .global plat_handle_exception
 .global bzero

.macro SAVE_REGS
	msr	spsel, #0
    sub sp, sp, #(22 * 8)

    stp x0, x1,   [sp, #(8*0)]
    stp x2, x3,   [sp, #(8*2)]
    stp x4, x5,   [sp, #(8*4)]
    stp x6, x7,   [sp, #(8*6)]
    stp x8, x9,   [sp, #(8*8)]
    stp x10, x11, [sp, #(8*10)]
    stp x12, x13, [sp, #(8*12)]
    stp x14, x15, [sp, #(8*14)]
    stp x16, x17, [sp, #(8*16)]
    stp x18, x19, [sp, #(8*18)]
    stp x29, x30, [sp, #(8*28)]
.endm

.macro RESTORE_REGS
    ldp x0, x1,   [sp, #(8*0)]
    ldp x2, x3,   [sp, #(8*2)]
    ldp x4, x5,   [sp, #(8*4)]
    ldp x6, x7,   [sp, #(8*6)]
    ldp x8, x9,   [sp, #(8*8)]
    ldp x10, x11, [sp, #(8*10)]
    ldp x12, x13, [sp, #(8*12)]
    ldp x14, x15, [sp, #(8*14)]
    ldp x16, x17, [sp, #(8*16)]
    ldp x18, x19, [sp, #(8*18)]
    ldp x29, x30, [sp, #(8*28)]

    add sp, sp, #(22 * 8)
.endm

/*
 * int plat_get_core_pos(uint64_t mpidr);
 * Clobber: x0
 */
func plat_get_core_pos
	ubfx    x0, x0, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
	ret
endfunc plat_get_core_pos

    /*
     * This macro calculates the base address of the current CPU's MP stack
     * using the plat_get_core_pos() index, the name of the stack storage
     * and the size of each stack
     * Out: X0 = physical address of stack base
     * Clobber: X30, X1, X2
     */
    .macro get_my_mp_stack _name, _size
    bl  plat_get_core_pos
    adrp    x2, (\_name + \_size)
    add x2, x2, :lo12:(\_name + \_size)
    mov x1, #\_size
    madd x0, x0, x1, x2
    .endm
/*
 * void plat_set_stack(void);
 * use SP_EL0
 * For the current CPU, this function sets the stack
 * pointer to a stack allocated in normal memory.
 */
func plat_set_stack
    mov x9, x30
    get_my_mp_stack normal_stack_top PLAT_STACK_SIZE
	msr	spsel, #0
	mov	sp, x0
    ret x9
endfunc plat_set_stack

/*
 * void plat_set_crash_stack(void);
 * use SP_EL0
 * For the current CPU, this function sets the stack
 * pointer to a stack allocated in normal memory.
 */
func plat_set_crash_stack
    mov x9, x30
    get_my_mp_stack crash_stack_top PLAT_STACK_SIZE
	msr	spsel, #0
	mov	sp, x0
    ret x9
endfunc plat_set_crash_stack

/*
 * void plat_handle_interrupt(void);
 * platform specific interrup handler
 */
func plat_handle_interrupt
    SAVE_REGS
    msr daifset, #1     /* Disable FIQ&IRQ */
    msr daifset, #2
    bl gic_handle
    msr daifclr, #1     /* Enable FIQ&IRQ */
    msr daifclr, #2
    RESTORE_REGS
    eret
endfunc plat_handle_interrupt

/*
 * void plat_handle_exception(void);
 * platform specific exception handler
 */
func plat_handle_exception
    b   do_unhandled_exception
endfunc plat_handle_exception

/* -----------------------------------------------------------------------
 * void bzero(void *mem, unsigned int length);
 *
 * Initialise a memory region to 0.
 * -----------------------------------------------------------------------
 */
func bzero
	add	x2, x0, x1
/* zero byte per byte for the start none 16-byte aligned mem*/
z_loopna:
	tst	x0, #0xf
	b.eq	z_loop16
	cmp	x0, x2
	b.eq	z_end
	strb	wzr, [x0], #1
	b	z_loopna
/* zero 16 bytes at a time */
z_loop16:
	sub	x3, x2, x0
	cmp	x3, #16
	b.lt	z_loop1
	stp	xzr, xzr, [x0], #16
	b	z_loop16
/* zero byte per byte */
z_loop1:
	cmp	x0, x2
	b.eq	z_end
	strb	wzr, [x0], #1
	b	z_loop1
z_end:	ret
endfunc bzero

/*
 * rom stack, core0 only
 */
#define STACK_ALIGN	6
.if ((PLAT_STACK_SIZE & ((1 << STACK_ALIGN) - 1)) <> 0)
  .error "Stack size not correctly aligned"
.endif
.section .stack, "aw", %nobits
.align STACK_ALIGN
normal_stack:
	.space (PLAT_STACK_SIZE*PLAT_CORE_COUNT - 4), 0
normal_stack_top:
	.space 8, 0

.section .stack, "aw", %nobits
.align STACK_ALIGN
crash_stack:
	.space (PLAT_STACK_SIZE*PLAT_CORE_COUNT - 4), 0
crash_stack_top:
	.space 8, 0
